/***************************************************
 @Name :        1196kk0
 @Time :        2025-05-26
 @Email :       2232506563@qq.com
 @Comment :     SG90舵机PWM初始化及动作函数
 @FileName :    drv_Servo.c
 @Version :     4.0
****************************************************/

#include "drv_Servo.h"

/***************************************
函数名：tim3_init
函数功能：初始化TIM3
输入参数：预装载值、预分频值
输出参数：无
其它：
****************************************/
void tim3_init(uint32_t count_circulation, uint32_t prescaler)
{
    TIM_TimeBaseInitTypeDef TIM_TimeBaseInitStruct;

    RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM3, ENABLE);

    TIM_TimeBaseInitStruct.TIM_Period = count_circulation;
    TIM_TimeBaseInitStruct.TIM_Prescaler = prescaler;
    TIM_TimeBaseInitStruct.TIM_CounterMode = TIM_CounterMode_Up;
    TIM_TimeBaseInitStruct.TIM_ClockDivision = TIM_CKD_DIV1;

    TIM_TimeBaseInit(TIM3, &TIM_TimeBaseInitStruct);
}

/***************************************
函数名：tim3_ch1_pa6_init
函数功能：初始化TIM3通道1PWM
输入参数：无
输出参数：无
其它：TIM3_CH1     PA6     左上
****************************************/
void tim3_ch1_pa6_init()
{
    GPIO_InitTypeDef GPIO_InitStruct;
    TIM_OCInitTypeDef TIM_OCInitStruct;

    RCC_AHB1PeriphClockCmd(RCC_AHB1Periph_GPIOA, ENABLE);

    GPIO_InitStruct.GPIO_Mode = GPIO_Mode_AF;
    GPIO_InitStruct.GPIO_OType = GPIO_OType_PP;
    GPIO_InitStruct.GPIO_Speed = GPIO_Speed_2MHz;
    GPIO_InitStruct.GPIO_PuPd = GPIO_PuPd_UP;
    GPIO_InitStruct.GPIO_Pin = GPIO_Pin_6;

    GPIO_Init(GPIOA, &GPIO_InitStruct);

    GPIO_PinAFConfig(GPIOA, GPIO_PinSource6, GPIO_AF_TIM3);

    TIM_OCInitStruct.TIM_OCMode = TIM_OCMode_PWM1;
    TIM_OCInitStruct.TIM_Pulse = 0;
    TIM_OCInitStruct.TIM_OCPolarity = TIM_OCPolarity_High;
    TIM_OCInitStruct.TIM_OutputState = TIM_OutputState_Enable;

    TIM_OC1Init(TIM3, &TIM_OCInitStruct);
    TIM_OC1PreloadConfig(TIM3, TIM_OCPreload_Enable);
    TIM_ARRPreloadConfig(TIM3, ENABLE);
    TIM_CCxCmd(TIM3, TIM_Channel_1, TIM_CCx_Enable);
    TIM_Cmd(TIM3, ENABLE);
}

/***************************************
函数名：tim3_ch2_pa7_init
函数功能：初始化TIM3通道2PWM
输入参数：无
输出参数：无
其它：TIM3_CH2     PA7     左下
****************************************/
void tim3_ch2_pa7_init()
{
    GPIO_InitTypeDef GPIO_InitStruct;
    TIM_OCInitTypeDef TIM_OCInitStruct;

    RCC_AHB1PeriphClockCmd(RCC_AHB1Periph_GPIOA, ENABLE);

    GPIO_InitStruct.GPIO_Mode = GPIO_Mode_AF;
    GPIO_InitStruct.GPIO_OType = GPIO_OType_PP;
    GPIO_InitStruct.GPIO_Speed = GPIO_Speed_2MHz;
    GPIO_InitStruct.GPIO_PuPd = GPIO_PuPd_UP;
    GPIO_InitStruct.GPIO_Pin = GPIO_Pin_7;

    GPIO_Init(GPIOA, &GPIO_InitStruct);

    GPIO_PinAFConfig(GPIOA, GPIO_PinSource7, GPIO_AF_TIM3);

    TIM_OCInitStruct.TIM_OCMode = TIM_OCMode_PWM1;
    TIM_OCInitStruct.TIM_Pulse = 0;
    TIM_OCInitStruct.TIM_OCPolarity = TIM_OCPolarity_High;
    TIM_OCInitStruct.TIM_OutputState = TIM_OutputState_Enable;

    TIM_OC2Init(TIM3, &TIM_OCInitStruct);
    TIM_OC2PreloadConfig(TIM3, TIM_OCPreload_Enable);
    TIM_ARRPreloadConfig(TIM3, ENABLE);
    TIM_CCxCmd(TIM3, TIM_Channel_2, TIM_CCx_Enable);
    TIM_Cmd(TIM3, ENABLE);
}

/***************************************
函数名：tim3_ch3_pc8_init
函数功能：初始化TIM3通道3PWM
输入参数：无
输出参数：无
其它：TIM3_CH3     PC8     右上
****************************************/
void tim3_ch3_pc8_init()
{
    GPIO_InitTypeDef GPIO_InitStruct;
    TIM_OCInitTypeDef TIM_OCInitStruct;

    RCC_AHB1PeriphClockCmd(RCC_AHB1Periph_GPIOC, ENABLE);

    GPIO_InitStruct.GPIO_Mode = GPIO_Mode_AF;
    GPIO_InitStruct.GPIO_OType = GPIO_OType_PP;
    GPIO_InitStruct.GPIO_Speed = GPIO_Speed_2MHz;
    GPIO_InitStruct.GPIO_PuPd = GPIO_PuPd_UP;
    GPIO_InitStruct.GPIO_Pin = GPIO_Pin_8;

    GPIO_Init(GPIOC, &GPIO_InitStruct);
    GPIO_PinAFConfig(GPIOC, GPIO_PinSource8, GPIO_AF_TIM3);

    TIM_OCInitStruct.TIM_OCMode = TIM_OCMode_PWM1;
    TIM_OCInitStruct.TIM_Pulse = 0;
    TIM_OCInitStruct.TIM_OCPolarity = TIM_OCPolarity_High;
    TIM_OCInitStruct.TIM_OutputState = TIM_OutputState_Enable;

    TIM_OC3Init(TIM3, &TIM_OCInitStruct);
    TIM_OC3PreloadConfig(TIM3, TIM_OCPreload_Enable);
    TIM_ARRPreloadConfig(TIM3, ENABLE);
    TIM_CCxCmd(TIM3, TIM_Channel_3, TIM_CCx_Enable);
    TIM_Cmd(TIM3, ENABLE);
}

/***************************************
函数名：tim3_ch4_pc9_init
函数功能：初始化TIM3通道4PWM
输入参数：无
输出参数：无
其它：TIM3_CH4     PC9     右下
****************************************/
void tim3_ch4_pc9_init()
{
    GPIO_InitTypeDef GPIO_InitStruct;
    TIM_OCInitTypeDef TIM_OCInitStruct;

    RCC_AHB1PeriphClockCmd(RCC_AHB1Periph_GPIOC, ENABLE);

    GPIO_InitStruct.GPIO_Mode = GPIO_Mode_AF;
    GPIO_InitStruct.GPIO_OType = GPIO_OType_PP;
    GPIO_InitStruct.GPIO_Speed = GPIO_Speed_2MHz;
    GPIO_InitStruct.GPIO_PuPd = GPIO_PuPd_UP;
    GPIO_InitStruct.GPIO_Pin = GPIO_Pin_9;

    GPIO_Init(GPIOC, &GPIO_InitStruct);

    GPIO_PinAFConfig(GPIOC, GPIO_PinSource9, GPIO_AF_TIM3);

    TIM_OCInitStruct.TIM_OCMode = TIM_OCMode_PWM1;
    TIM_OCInitStruct.TIM_Pulse = 0;
    TIM_OCInitStruct.TIM_OCPolarity = TIM_OCPolarity_High;
    TIM_OCInitStruct.TIM_OutputState = TIM_OutputState_Enable;

    TIM_OC4Init(TIM3, &TIM_OCInitStruct);
    TIM_OC4PreloadConfig(TIM3, TIM_OCPreload_Enable);
    TIM_ARRPreloadConfig(TIM3, ENABLE);
    TIM_CCxCmd(TIM3, TIM_Channel_4, TIM_CCx_Enable);
    TIM_Cmd(TIM3, ENABLE);
}

/***************************************
函数名：tim5_ch2_pa1_init
函数功能：初始化TIM5通道2PWM
输入参数：无
输出参数：无
其它：TIM5_CH2     PA1     尾巴
    分频=84：计数1次需要的时间是1us
	装载值=20000；需要计的次数；
	定时时间:20ms = 1*20000=20000us
****************************************/
void tim5_ch2_pa1_init()
{
    TIM_TimeBaseInitTypeDef TIM_TimeBaseInitStruct;
    GPIO_InitTypeDef GPIO_InitStruct;
    TIM_OCInitTypeDef TIM_OCInitStruct;

    RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM5, ENABLE);
    RCC_AHB1PeriphClockCmd(RCC_AHB1Periph_GPIOA, ENABLE);

    TIM_TimeBaseInitStruct.TIM_Period = 20000-1;
    TIM_TimeBaseInitStruct.TIM_Prescaler = 84-1;
    TIM_TimeBaseInitStruct.TIM_CounterMode = TIM_CounterMode_Up;
    TIM_TimeBaseInitStruct.TIM_ClockDivision = TIM_CKD_DIV1;

    TIM_TimeBaseInit(TIM5, &TIM_TimeBaseInitStruct);

    GPIO_InitStruct.GPIO_Mode = GPIO_Mode_AF;
    GPIO_InitStruct.GPIO_OType = GPIO_OType_PP;
    GPIO_InitStruct.GPIO_Speed = GPIO_Speed_2MHz;
    GPIO_InitStruct.GPIO_PuPd = GPIO_PuPd_UP;
    GPIO_InitStruct.GPIO_Pin = GPIO_Pin_1;

    GPIO_Init(GPIOA, &GPIO_InitStruct);

    GPIO_PinAFConfig(GPIOA, GPIO_PinSource1, GPIO_AF_TIM5);

    TIM_OCInitStruct.TIM_OCMode = TIM_OCMode_PWM1;
    TIM_OCInitStruct.TIM_Pulse = 0;
    TIM_OCInitStruct.TIM_OCPolarity = TIM_OCPolarity_High;
    TIM_OCInitStruct.TIM_OutputState = TIM_OutputState_Enable;

    TIM_OC2Init(TIM5, &TIM_OCInitStruct);
    TIM_OC2PreloadConfig(TIM5, TIM_OCPreload_Enable);       //使能通道预装载功能
    TIM_ARRPreloadConfig(TIM5, ENABLE);                     //重装载值使能
    TIM_CCxCmd(TIM5, TIM_Channel_2, TIM_CCx_Enable);        //使能输出功能
    TIM_Cmd(TIM5, ENABLE);
}

/***************************************
函数名：set_angle
函数功能：设置舵机旋转角度
输入参数：angle 角度值0°~180°   position 脚
输出参数：无
其它：
****************************************/
void set_angle(uint8_t angle, uint16_t position)
{
    uint16_t pulse;
    if (angle > 180) 
        angle = 180;
    pulse = 500 + (uint32_t)(2000 * angle) / 180;
    switch (position) {
        case LEFT_UP:
            TIM_SetCompare1(TIM3, pulse);
            break;
        case LEFT_DOWN:
            TIM_SetCompare2(TIM3, pulse);
            break;
        case RIGHT_UP:
            TIM_SetCompare3(TIM3, pulse);
            break;
        case RIGHT_DOWN:
            TIM_SetCompare4(TIM3, pulse);
            break;
        case TAIL:
            TIM_SetCompare2(TIM5, pulse);
        default:
            break;
    }
}

/********* 全局变量 *********/
uint8_t leg_angle[4] = {90, 90, 90, 90};

/***************************************
函数名：smooth_moving
函数功能：舵机平滑移动
输入参数：new_angle 角度值0°~180°   position 脚
输出参数：无
其它：
****************************************/
void smooth_moving(uint8_t new_angle, uint16_t position)
{
    _Bool is_modifying;
    while (1)
    {
        is_modifying = 0;
        if (position & 8)
        {
            (leg_angle[0] > new_angle) ? (leg_angle[0] -= 2) : (leg_angle[0] += 2);
            if (leg_angle[0] != new_angle) is_modifying = 1;
        }
        if (position & 4)
        {
            (leg_angle[1] > new_angle) ? (leg_angle[1] -= 2) : (leg_angle[1] += 2);
            if (leg_angle[1] != new_angle) is_modifying = 1;
        }
        if (position & 2)
        {
            (leg_angle[2] > new_angle) ? (leg_angle[2] -= 2) : (leg_angle[2] += 2);
            if (leg_angle[2] != new_angle) is_modifying = 1;
        }
        if (position & 1)
        {
            (leg_angle[3] > new_angle) ? (leg_angle[3] -= 2) : (leg_angle[3] += 2);
            if (leg_angle[3] != new_angle) is_modifying = 1;
        }
        set_angle(leg_angle[0], position & 8);
        set_angle(leg_angle[1], position & 4);
        set_angle(180 - leg_angle[2], position & 2);
        set_angle(180 - leg_angle[3], position & 1);
        delay_ms(15);
        if (!is_modifying) break;
    }
}

/***************************************
函数名：Dog_Stand
函数功能：机器狗站立
输入参数：无
输出参数：无
其它：
****************************************/
void Dog_Stand(void)
{
    smooth_moving(90, LEFT_UP | LEFT_DOWN | RIGHT_UP | RIGHT_DOWN);
}

/***************************************
函数名：Dog_Sit
函数功能：机器狗坐下
输入参数：无
输出参数：无
其它：
****************************************/
void Dog_Sit(void)
{
	set_angle(60 , TAIL);
    smooth_moving(90, LEFT_UP);
    smooth_moving(90, RIGHT_UP);
    delay_ms(200);
	set_angle(120 , TAIL);
    smooth_moving(30, LEFT_DOWN | RIGHT_DOWN);
    delay_ms(200);
}

/***************************************
函数名：Dog_Lie_down
函数功能：机器狗趴下
输入参数：无
输出参数：无
其它：
****************************************/
void Dog_Lie_down(void)
{
    smooth_moving(0 , LEFT_UP);
    smooth_moving(0 , RIGHT_UP);
    smooth_moving(180 , LEFT_DOWN);
    smooth_moving(180 , RIGHT_DOWN);
}

/***************************************
函数名：Dog_Handshake
函数功能：机器狗握手
输入参数：无
输出参数：无
其它：
****************************************/
void Dog_Handshake(void)
{
	smooth_moving(20,RIGHT_UP);
    set_angle(60 , TAIL);
	delay_ms(200);
	smooth_moving(50,RIGHT_UP);
    set_angle(120 , TAIL);
	delay_ms(200);
}

/***************************************
函数名：Dog_Move_forward
函数功能：机器狗前进
输入参数：无
输出参数：无
其它：
****************************************/
void Dog_Move_forward(void)
{
#if 1
        smooth_moving(80 , LEFT_UP);
        smooth_moving(80 , RIGHT_DOWN);
        delay_ms(5);
        smooth_moving(100 , LEFT_UP);
        smooth_moving(100 , RIGHT_DOWN);
        delay_ms(5);
        smooth_moving(80 , RIGHT_UP);
        smooth_moving(80 , LEFT_DOWN);
        delay_ms(5);
        smooth_moving(100 , RIGHT_UP);
        smooth_moving(100, LEFT_DOWN);
        delay_ms(5);
#endif

#if 0
        smooth_moving(110,RIGHT_DOWN);
        delay_ms(10);
        smooth_moving(70,LEFT_DOWN);
        delay_ms(10);
        smooth_moving(70,RIGHT_UP);
         delay_ms(10);
        smooth_moving(110,LEFT_UP);
        delay_ms(10);
        smooth_moving(70,RIGHT_DOWN);
        delay_ms(10);
        smooth_moving(110,LEFT_DOWN);
        delay_ms(10);
        smooth_moving(110,RIGHT_UP);
        delay_ms(10);
        smooth_moving(70,LEFT_UP);
        delay_ms(10);
#endif
}

/***************************************
函数名：Dog_Move_back
函数功能：机器狗后退
输入参数：无
输出参数：无
其它：
****************************************/
void Dog_Move_back(void)
{
    smooth_moving(130,RIGHT_UP);
    smooth_moving(130,LEFT_DOWN);
    delay_ms(10);
    smooth_moving(90,RIGHT_UP);
    smooth_moving(90,LEFT_DOWN);
    delay_ms(10);
    smooth_moving(130,LEFT_UP);
    smooth_moving(130,RIGHT_DOWN);
    delay_ms(10);
    smooth_moving(90,LEFT_UP);
    smooth_moving(90,RIGHT_DOWN);
    delay_ms(10);
}

/***************************************
函数名：Dog_Turn_left
函数功能：机器狗左转
输入参数：无
输出参数：无
其它：
****************************************/
void Dog_Turn_left(void)
{
        smooth_moving(80 , LEFT_UP);
        smooth_moving(70 , RIGHT_DOWN);
        delay_ms(5);
        smooth_moving(100 , LEFT_UP);
        smooth_moving(110 , RIGHT_DOWN);
        delay_ms(5);
        smooth_moving(70 , RIGHT_UP);
        smooth_moving(80 , LEFT_DOWN);
        delay_ms(5);
        smooth_moving(110 , RIGHT_UP);
        smooth_moving(100 , LEFT_DOWN);
        delay_ms(5);
}

/***************************************
函数名：Dog_Turn_right
函数功能：机器狗右转
输入参数：无
输出参数：无
其它：
****************************************/
void Dog_Turn_right(void)
{
        smooth_moving(70 , LEFT_UP);
        smooth_moving(80 , RIGHT_DOWN);
        delay_ms(5);
        smooth_moving(120 , LEFT_UP);
        smooth_moving(100 , RIGHT_DOWN);
        delay_ms(5);
        smooth_moving(80 , RIGHT_UP);
        smooth_moving(70 , LEFT_DOWN);
        delay_ms(5);
        smooth_moving(100 , RIGHT_UP);
        smooth_moving(120, LEFT_DOWN);
        delay_ms(5);
}

/***************************************
函数名：Dog_Dance
函数功能：机器狗跳舞
输入参数：无
输出参数：无
其它：
****************************************/
void Dog_Dance(void)
{
    smooth_moving(100 , RIGHT_UP);
    smooth_moving(100, LEFT_DOWN);
    set_angle(60 , TAIL);
    delay_ms(10);
    smooth_moving(80 , RIGHT_UP);
    smooth_moving(80 , LEFT_DOWN);
    set_angle(120 , TAIL);
    delay_ms(10);
    smooth_moving(100 , LEFT_UP);
    smooth_moving(100 , RIGHT_DOWN);
    set_angle(60 , TAIL);
    delay_ms(10);
    smooth_moving(80 , LEFT_UP);
    smooth_moving(80 , RIGHT_DOWN);
    set_angle(120 , TAIL);
    delay_ms(10);
}

/***************************************
函数名：Dog_Push_up
函数功能：机器狗爬行
输入参数：无
输出参数：无
其它：
****************************************/
void Dog_Push_up(void)
{
    smooth_moving(160 , RIGHT_DOWN);
    smooth_moving(160 , LEFT_DOWN);
    smooth_moving(30 , RIGHT_UP);
    smooth_moving(30 , LEFT_UP);
    delay_ms(20);
    smooth_moving(60 , RIGHT_UP);
    smooth_moving(60, LEFT_UP);
	delay_ms(20);
}


